# @Time : 2021/4/17 9:51
# @Time : 2021/4/17 9:51
# @Author : Fioman
# @Phone : 13149920693
"""
线阵相机标定工具,主要处理线阵相机的标定的逻辑
"""
import time

import gevent

from db_tools.db_handler import DbHandler
from tools.hd_logger import HdLogger
from tools.save_image_to_device import open_line_cam_cal_image_dir
from tools.send_to_ui_tools import send_line_camera_cal_state_to_ui
from tools.common_tools import image_to_string
from settings import CALIBRATION_DIR
import cv2 as cv
import numpy as np
import os
from tools.fioman_math_tools import calc_calibration_mtx, calc_calibration_mtx_new
from tools.fioman_math_tools import pix_pos_to_robot
from tools.config_params import ConfigParams

cp = ConfigParams()


class LineCameraCalTool(object):
    """
    线阵相机的标定类
    """

    def __init__(self, imagejoint, cbjZmc, cutterZmc):
        self.db = DbHandler()
        self.logger = HdLogger()
        self.imageJoint = imagejoint
        self.camZmc = imagejoint.camZmc
        self.cbjZmc = cbjZmc
        self.cutterZmc = cutterZmc
        self.imageKeepDir = CALIBRATION_DIR
        self.camName = ""

    def delete_old_image(self):
        """
        删除旧的标定目录下的文件
        :return:
        """
        for root, dirs, files in os.walk(self.imageKeepDir):
            for file in files:
                if root != self.imageKeepDir:
                    continue
                os.remove(os.path.join(self.imageKeepDir, file))

    def get_image_for_line_cam_cal(self):
        """
        获取图片
        :return:
        """
        return self.imageJoint.get_image_from_line_cam()

    def web_request_start_device(self):
        """
        标定的时候请求启动设备
        :return:
        """
        catchPos = cp.calcCatchPos
        ret = self.camZmc.set_modbus_float(108, 1, [catchPos])
        if ret != 0:
            return "铡刀控制器通信异常,设备启动失败"
        ret = self.cutterZmc.cutter_zmc_start()  # 设备启动
        if ret != 0:
            return "铡刀控制器通信异常,设备启动失败"
        # self.cutterZmc.set_two_board_joint_flag()
        msgVlue = {
            "val": "updateStartState",
            "startStateVisble": True,
            "info": "设备启动中...",
        }
        send_line_camera_cal_state_to_ui(msgVlue)
        self.startStateCheck = True  # 设备状态检测状态为True
        deviceStartTime = time.time()
        while self.startStateCheck:
            timeCheck = time.time() - deviceStartTime
            if timeCheck > 60:
                msgVlue = {
                    "val": "updateStartState",
                    "startModalVisble": True,
                    "startInfo": "设备启动超时..."
                }
                send_line_camera_cal_state_to_ui(msgVlue)
                return "resetTimeout"

            _, val1 = self.camZmc.is_running()
            _, val2 = self.cbjZmc.is_running()
            _, val3 = self.cutterZmc.is_running()
            info1 = "1. 上料启动中..." if val1 == 0 else "1. 上料启动成功!"
            info2 = "2. 裁切机启动中..." if val2 == 0 else "2. 裁切机启动成功!"
            info3 = "3. 铡刀启动中..." if val3 == 0 else "3. 铡刀启动成功!"
            if val1 == 1 and val2 == 1 and val3 == 1:
                self.startStateCheck = False
                msgVlue = {
                    "val": "updateStartState",
                    "startModalVisble": False,
                    "startInfo": "设备启动成功..."
                }
                send_line_camera_cal_state_to_ui(msgVlue)
                return "ok"
            else:
                msgVlue = {
                    "val": "updateStartState",
                    "startModalVisble": True,
                    "startInfo": info1 + " " + info2 + " " + info3
                }
                send_line_camera_cal_state_to_ui(msgVlue)
            gevent.sleep(0.3)

    def web_request_get_image_for_line_cam_cal(self):
        """
        前端界面请求抓取图片,获取标定使用的图片.
        :return:
        """
        requestStart = time.time()
        while True:
            if time.time() - requestStart > 20:
                return "没有检测到有板信号,未收到允许拍照信号,请确保板在起点位置,并触发了传感器"
            ret, val = self.camZmc.get_allow_get_pic_state()
            if ret != 0:
                return "上料控制器通信异常,读取允许拍照信号失败"
            if val == 1:
                break
            else:
                gevent.sleep(0.5)
                continue
        if val == 1:
            res, img = self.get_image_for_line_cam_cal()
            if res != "ok":
                return res
            else:
                # 先删除目录下的所有的图片
                self.delete_old_image()
                fileName = "{}.bmp".format(time.strftime("%Y-%m-%d_%H-%M-%S", time.localtime()))
                cv.imwrite(os.path.join(self.imageKeepDir, fileName), img)
                ret = self.camZmc.img_result_ok()
                if ret != 0:
                    return "上料控制器通信异常,读取允许拍照信号失败"
                else:
                    imageSend = image_to_string(img)
                    msgValue = {
                        "val": "updateLineCamCalImage",
                        "calImageSrc": imageSend
                    }
                    send_line_camera_cal_state_to_ui(msgValue)
                return "ok"

    def web_request_calc_trans_mtx(self, calPointsPixInfo):
        """
        前端界面点击了标定按钮.传过来的都是图像坐标,是一个一维列表
        :param calPointsPix:
        :return:2664, 1397, 2683, 281, 625, 267, 621, 1194
        """
        calPointsPix = calPointsPixInfo
        # self.camName = calPointsPixInfo[-1]
        pixPos = [int(x) for x in calPointsPix]
        if 0 in pixPos:
            return "图像像素坐标不能是空,请重新填写!"

        # pixPosX = pixPos[1:len(pixPos) + 1:2]
        # pixPosX = pixPos[0:len(pixPos) + 1:2]
        # if self.camName == "left":
        #     pixPosXIsRight = [(y <= cp.middleJointPos) for y in pixPosX]
        # else:
        #     pixPosXIsRight = [(y >= cp.middleJointPos) for y in pixPosX]
        # print(pixPosXIsRight)
        # if False in pixPosXIsRight:
        #     return "像素坐标错误,标定的相机:{},标定使用的像素点:{},拼接处:{},请确保标定左相机的时候,取的点的坐标都在拼接处的左侧,标定的右相机的时候,取的点都在拼接处的右侧". \
        #         format(self.camName, pixPos, cp.middleJointPos)

        imagePos = np.reshape(pixPos, (4, 2))
        res, transMtxData = self.db.get_trans_mtx_data()
        if res != "ok":
            return transMtxData
        p1 = transMtxData.get('point_1')
        p2 = transMtxData.get('point_2')
        p3 = transMtxData.get('point_3')
        p4 = transMtxData.get('point_4')
        offsetX = transMtxData.get('offsetX')
        offsetY = transMtxData.get('offsetY')
        if None in [p1, p2, p3, p4, offsetX, offsetY]:
            retInfo = ""
            if p1 is None:
                retInfo += "点1裁切机坐标获取失败 "
            if p2 is None:
                retInfo += "点2裁切机坐标获取失败 "
            if p3 is None:
                retInfo += "点3裁切机坐标获取失败 "
            if p4 is None:
                retInfo += "点4裁切机坐标获取失败 "
            if offsetX is None:
                retInfo += "激光偏移值X获取失败 "
            if offsetY is None:
                retInfo += "激光偏移值Y获取失败"
            return retInfo
        offset = [offsetX, offsetY]
        robotPos = [p1, p2, p3, p4]
        # 刀头激光偏移加进去
        robotPos = np.add(robotPos, offset).tolist()  # 加上激光偏移的第四个点.
        self.transMtx = calc_calibration_mtx(robotPos[:3], imagePos[:3]).tolist()
        # 根据第四个点计算误差
        # if self.camName == "left":
        #     calcPoint4Robot = pix_pos_to_robot(imagePos[3], [self.transMtx, None])
        # else:
        #     calcPoint4Robot = pix_pos_to_robot(imagePos[3], [None, self.transMtx])
        calcPoint4Robot = pix_pos_to_robot(imagePos[3], self.transMtx)
        matrixError = np.subtract(calcPoint4Robot, robotPos[3])
        matrixError = [round(x, 4) for x in matrixError]
        msgValue = {
            "val": "updateCalibrationMtx",
            "calibrationError": matrixError,
            "calibrationMtx": self.transMtx,
            "calibrationOffsetUsed": offset,
            "calibarionRobotUsed": robotPos,
        }
        send_line_camera_cal_state_to_ui(msgValue)
        return "ok"

    def web_request_calc_trans_mtx_nine_points(self, calPointsPix):
        pixPos = [int(x) for x in calPointsPix]
        if 0 in pixPos:
            return "图像像素坐标不能是空,请重新填写"
        imagePos = np.reshape(pixPos, (9, 2))
        res, transMtxData = self.db.get_trans_mtx_data()
        if res != "ok":
            return transMtxData
        p1 = transMtxData.get('point_1')
        p2 = transMtxData.get('point_2')
        p3 = transMtxData.get('point_3')
        p4 = transMtxData.get('point_4')
        p5 = transMtxData.get('point_5')
        p6 = transMtxData.get('point_6')
        p7 = transMtxData.get('point_7')
        p8 = transMtxData.get('point_8')
        p9 = transMtxData.get('point_9')
        offsetX = transMtxData.get('offsetX')
        offsetY = transMtxData.get('offsetY')
        if None in [p1, p2, p3, p4, p5, p6, p7, p8, p9, offsetX, offsetY]:
            retInfo = ""
            if p1 is None:
                retInfo += "点1裁切机坐标获取失败 "
            if p2 is None:
                retInfo += "点2裁切机坐标获取失败 "
            if p3 is None:
                retInfo += "点3裁切机坐标获取失败 "
            if p4 is None:
                retInfo += "点4裁切机坐标获取失败 "
            if p5 is None:
                retInfo += "点5裁切机坐标获取失败"
            if p6 is None:
                retInfo += "点6裁切机坐标获取失败 "
            if p7 is None:
                retInfo += "点7裁切机坐标获取失败 "
            if p8 is None:
                retInfo += "点8裁切机坐标获取失败 "
            if p9 is None:
                retInfo += "点9裁切机坐标获取失败"

            if offsetX is None:
                retInfo += "激光偏移值X获取失败 "
            if offsetY is None:
                retInfo += "激光偏移值Y获取失败"
            return retInfo

        offset = [offsetX, offsetY]
        robotPosWithoutOffset = [p1, p2, p3, p4, p5, p6, p7, p8, p9]
        # 刀头激光偏移加进去
        robotPos = np.add(robotPosWithoutOffset, offset).tolist()
        self.transMtx = calc_calibration_mtx_new(imagePos[:8], robotPos[:8])
        if self.transMtx is None:
            return "标定失败,图像像素坐标和裁切机坐标某个点有误,请重新确认一下!"
        # 根据第四个点计算误差
        calcPoint4Robot = pix_pos_to_robot(imagePos[8], self.transMtx)
        matrixError = np.subtract(calcPoint4Robot, robotPos[8])
        matrixError = [round(x, 4) for x in matrixError]
        mtxSend = self.transMtx.tolist()
        msgValue = {
            "val": "updateCalibrationMtx",
            "calibrationError": matrixError,
            "calibrationMtx": mtxSend,
            "calibrationOffsetUsed": offset,
            "calibarionRobotUsed": robotPosWithoutOffset,
        }
        send_line_camera_cal_state_to_ui(msgValue)
        return "ok"

    def web_request_save_trans_mtx(self):
        """
        前端界面点击了保存标定矩阵,这个时候是在点击标定只有才有的值.
        :return:
        """
        try:
            if not isinstance(self.transMtx,type([])):
              transMtx = self.transMtx.tolist()
            else:
              transMtx = self.transMtx
        except Exception as e:
            errorInfo = "in LineCameraCalTool.web_request_save_trans_mtx() error: {}".format(str(e))
            print(errorInfo)
            self.logger.debug(errorInfo)
            return "没有要保存的标定矩阵,请先点击标定!"
        # if self.camName == "left":
        #     newData = {
        #         "mtxLeft": transMtx
        #     }
        # elif self.camName == "right":
        #     newData = {
        #         "mtxRight": transMtx
        #     }
        # else:
        #     return "相机名称错误"
        newData={
            "transMtx": transMtx,
        }
        res, info = self.db.save_trans_mtx_data(newData)
        return "ok" if res == "ok" else info

    def web_request_verify_trans_mtx(self, verifyPoint):
        """
        前端界面点击了验证标定矩阵,开始根据这个东西去计算标定矩阵.
        :param verifyPoint:验证的点的像素坐标
        :return:
        """
        res, transMtxData = self.db.get_trans_mtx_data()
        if res != "ok":
            return transMtxData
        # mtxLeft = transMtxData.get("mtxLeft")
        # mtxRight = transMtxData.get("mtxRight")
        transMtx = transMtxData.get("transMtx")
        if transMtx is None :
            return "标定矩阵获取失败,请先标定,并保存标定结果再进行验证!"
        verifyPoint = [int(x) for x in verifyPoint]
        calcX, calcY = pix_pos_to_robot(verifyPoint, transMtx)
        offsetX = transMtxData.get("offsetX")
        offsetY = transMtxData.get("offsetY")
        moveX = float(calcX) - float(offsetX)
        moveY = float(calcY) - float(offsetY)
        moveFourPos = [moveX, moveY, 0, 0]
        self.cbjZmc.move_abs_four_axis(moveFourPos)
        return "ok"

    def web_request_move_to_point(self, movePos):
        """
        前端界面点击移动到标定的点的裁切机坐标.
        :param movePos: 要移动的保存到数据库中的哪个点.
        :return:
        """

        res, transMtxData = self.db.get_trans_mtx_data()
        if res != "ok":
            return transMtxData
        p1 = transMtxData.get('point_1')
        p2 = transMtxData.get('point_2')
        p3 = transMtxData.get('point_3')
        p4 = transMtxData.get('point_4')
        p5 = transMtxData.get('point_5')
        p6 = transMtxData.get('point_6')
        p7 = transMtxData.get('point_7')
        p8 = transMtxData.get('point_8')
        p9 = transMtxData.get('point_9')
        if movePos == "1":
            movePoint = p1
        elif movePos == "2":
            movePoint = p2
        elif movePos == "3":
            movePoint = p3
        elif movePos == "4":
            movePoint = p4
        elif movePos == "5":
            movePoint = p5
        elif movePos == "6":
            movePoint = p6
        elif movePos == "7":
            movePoint = p7
        elif movePos == "8":
            movePoint = p8
        elif movePos == "9":
            movePoint = p9
        else:
            movePoint = p4
        if movePoint is None:
            return "点{}裁切机坐标未保存进数据库".format(movePos)
        moveX, moveY = movePoint
        moveFourPos = [float(moveX), float(moveY), 0, 0]
        self.cbjZmc.move_abs_four_axis(moveFourPos)
        return "ok"

    def web_request_open_line_cam_cal_image_dir(self):
        """
        点击打开标定图片目录
        :return:
        """
        return open_line_cam_cal_image_dir()


if __name__ == '__main__':
    pixPos = ["1789", "3216", "2832", "3186", "2628", "580", "1833", "601", "right"]
    pixPosX = pixPos[0:len(pixPos) + 1:2]
    print(pixPosX)
